#include "cube.h"
#include <vector>

const Point Cube::points[8] = {	Point(-0.5,-0.5,0.5),Point(0.5,-0.5,0.5),Point(0.5,0.5,0.5),Point(-0.5,0.5,0.5),
								Point(-0.5,-0.5,-0.5),Point(0.5,-0.5,-0.5),Point(0.5,0.5,-0.5),Point(-0.5,0.5,-0.5)	};

Cube::Cube(){
	// Se supone que se incializa luego
}

Cube::Cube(const Point& p1,const Point p2){
	// TODO No verifica que p1 y p2 no formen un cubo
	createCubePoints(p1,p2);
	calculateNormals(_normals);
}

Cube& Cube::translate(float dx,float dy,float dz){
	Matrix4x4 t1(dx,dy,dz);

	for(unsigned int c=0;c<4;c++){
		frontFace()[c]=t1*frontFace()[c];
		backFace()[c]=t1*backFace()[c];
	}

	return *this;
}

Point Cube::center()const{
	return ((frontFace()[0]-backFace()[2])*0.5) + backFace()[2];
}

float Cube::width()const{
	return (frontFace()[0]-frontFace()[1]).mod();
}

float Cube::height()const{
	return (frontFace()[0]-backFace()[0]).mod();
}

float Cube::depth()const{
	return (frontFace()[0]-frontFace()[3]).mod();
}


Cube& Cube::center(const Point& center){
	Point p = center - this->center();
	this->translate(p.x1,p.x2,p.x3);

	return *this;
}

const Point* Cube::getNormalizedPoints(){
	return points;
}

void Cube::rotate(float angle,const Point& center,const Point& axis){
	Matrix4x4 t1(-center.x1,-center.x2,-center.x3);
	Matrix4x4 r1(angle,axis);
	Matrix4x4 t2(center.x1,center.x2,center.x3);
	Matrix4x4 m;
	m*=t1;
	m*=r1;
	m*=t2;

	for(unsigned int c=0;c<4;c++){
		frontFace()[c]=m*frontFace()[c];
		backFace()[c]=m*backFace()[c];
		normals()[c]=m*backFace()[c];
	}

	calculateNormals(_normals);
}

bool Cube::intersect(const Cube& c2)const{
	// Se trata de ver si el objeto esta fuera del cubo,
	// esto sucede si sus vertices estan completamente detras de algun plano.
	// Caso contrario la caja esta total o parcialmente adentro o la caja contiene
	// al cubo.

	unsigned int pointsIndex[6] = { 4,0,1,0,2,0 };

	// Cuento la cantidad de vertices que estan dentro del cubo
	int totalVerticesIn = 0;

	const Point* boxVertices = c2.frontFace();

	// Para cada plano del cubo
	for(unsigned int j=0;j<6;j++){
		int vertexInCount = 8; // Supongo que estan todos adentro al comienzo
		int isIn = 1; // Supongo que esta adentro

		// Para cada vertice de la caja
		for(unsigned int i=0;i<8;i++){
			Point p= boxVertices[i]-_vertices[pointsIndex[j]];
			float val = (p*_normals[j]);

			if(val>0){
				isIn = 0;
				vertexInCount--;
			}
		}

		// El objeto esta totalmente detras de un plano
		// por lo tanto esta fuera del cubo
		if(!vertexInCount)
			return false;

		totalVerticesIn += isIn;
	}

	// La caja o al menos uno de sus vertices esta dentro(o lo contiene).
	return true;
}

float Cube::minZ()const{
	return min(3);
}

float Cube::maxZ()const{
	return max(3);
}

float Cube::minX()const{
	return min(1);
}

float Cube::maxX()const{
	return max(1);
}

float Cube::minY()const{
	return min(2);
}

float Cube::maxY()const{
	return max(2);
}

float Cube::max(unsigned int coord)const{
	float _max = frontFace()[0][coord];

	for(unsigned int c=0;c<4;c++){
		if(frontFace()[c][coord]>_max)
			_max = frontFace()[c][coord];
		if(backFace()[c][coord]>_max)
			_max = backFace()[c][coord];
	}

	return _max;
}

float Cube::min(unsigned int coord)const{
	float _min = frontFace()[0][coord];

	for(unsigned int c=0;c<4;c++){
		if(frontFace()[c][coord]<_min)
			_min = frontFace()[c][coord];
		if(backFace()[c][coord]<_min)
			_min = backFace()[c][coord];
	}

	return _min;
}

bool Cube::operator <(const Cube& cube)const{
	return(this->minZ()<cube.minZ());
}

Cube& Cube::extend(float dx,float dy,float dz){
	std::set<unsigned int> indices;
	Point normal[6];
	Point dir(dx,dy,dz);
	calculateNormals(normal);

	for(unsigned int j=0;j<6;j++){
		if(dir*normal[j]>0){
			getFaceIndices(indices,j);
		}
	}


	for(std::set<unsigned int>::iterator it=indices.begin();it!=indices.end();++it){
		unsigned int val = *it;
		Matrix4x4 t1(dx,dy,dz);
		if(val<4)
			frontFace()[val]=t1*frontFace()[val];
		else
			backFace()[val-4]=t1*backFace()[val-4];
	}

	calculateNormals(_normals);

	return *this;
}

Cube& Cube::shrink(float dx,float dy,float dz){
	std::set<unsigned int> indices;
	Point normal[6];
	Point dir(-dx,-dy,-dz);
	calculateNormals(normal);

	for(unsigned int j=0;j<6;j++){
		if(dir*normal[j]<0){
			getFaceIndices(indices,j);
		}
	}


	for(std::set<unsigned int>::iterator it=indices.begin();it!=indices.end();++it){
		unsigned int val = *it;
		Matrix4x4 t1(-dx,-dy,-dz);
		if(val<4)
			frontFace()[val]=t1*frontFace()[val];
		else
			backFace()[val-4]=t1*backFace()[val-4];
	}

	calculateNormals(_normals);

	return *this;
}

void Cube::getFaceIndices(std::set<unsigned int>& indices,unsigned int faceNumber){
	switch(faceNumber){
		case 0:
			indices.insert(4);
			indices.insert(5);
			indices.insert(6);
			indices.insert(7);
			break;
		case 1:
			indices.insert(0);
			indices.insert(1);
			indices.insert(2);
			indices.insert(3);
			break;
		case 2:
			indices.insert(1);
			indices.insert(2);
			indices.insert(5);
			indices.insert(6);
			break;
		case 3:
			indices.insert(0);
			indices.insert(3);
			indices.insert(4);
			indices.insert(7);
			break;
		case 4:
			indices.insert(2);
			indices.insert(3);
			indices.insert(6);
			indices.insert(7);
			break;
		case 5:
			indices.insert(0);
			indices.insert(1);
			indices.insert(4);
			indices.insert(5);
			break;
	}
}

void Cube::calculateNormals(Point* normal)const{
	Point nullPoint;

	normal[0]  = backFace()[0]-frontFace()[0];
	normal[2]  = frontFace()[1]-frontFace()[0];
	normal[4]  = frontFace()[2]-frontFace()[1];
	normal[1]  = normal[0]*-1;
	normal[3]  = normal[2]*-1;
	normal[5]  = normal[4]*-1;
}

Cube& Cube::advance(const Point& direction,float amount){
	Point pm = direction*amount;
	this->translate(pm.x1,pm.x2,pm.x3);

	return *this;
}

Cube& Cube::advance(const Point& direction){
	Point pm = direction;
	this->translate(pm.x1,pm.x2,pm.x3);

	return *this;
}


void Cube::createCubePoints(const Point& p1,const Point p2){
	if(p1.x3>p2.x3){
		frontFace()[0]=Point(p1.x1,p1.x2,p1.x3);
		frontFace()[1]=Point(p2.x1,p1.x2,p1.x3);
		frontFace()[2]=Point(p2.x1,p2.x2,p1.x3);
		frontFace()[3]=Point(p1.x1,p2.x2,p1.x3);

		backFace()[0]=Point(p1.x1,p1.x2,p2.x3);
		backFace()[1]=Point(p2.x1,p1.x2,p2.x3);
		backFace()[2]=Point(p2.x1,p2.x2,p2.x3);
		backFace()[3]=Point(p1.x1,p2.x2,p2.x3);
	}else{
		backFace()[0]=Point(p1.x1,p1.x2,p1.x3);
		backFace()[1]=Point(p2.x1,p1.x2,p1.x3);
		backFace()[2]=Point(p2.x1,p2.x2,p1.x3);
		backFace()[3]=Point(p1.x1,p2.x2,p1.x3);

		frontFace()[0]=Point(p1.x1,p1.x2,p2.x3);
		frontFace()[1]=Point(p2.x1,p1.x2,p2.x3);
		frontFace()[2]=Point(p2.x1,p2.x2,p2.x3);
		frontFace()[3]=Point(p1.x1,p2.x2,p2.x3);
	}
}

